﻿using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace NinjaToolbox.PhysicsSystem
{
    public class BVHTree
    {
        public struct NodeBuildInfo
        {
            public int ElementStartIndex { get; set; }
            public int ElementEndIndex { get; set; }
        }

        public class Node
        {
            public Node LChildren { get; set; }
            public Node RChildren { get; set; }
            public PhysicsBase[] ItemsArray { get; set; }

            public Bounds Bounds { get; set; }

            public bool IsLeaf { get { return ItemsArray != null; } }
        }

        Node mRoot;
        List<PhysicsBase> mPrimList;
        Queue<Node> mQueryCacheQueue;

        public Node Root { get { return mRoot; } }
        public List<PhysicsBase> PrimList { get { return mPrimList; } }


        public BVHTree()
        {
            mPrimList = new List<PhysicsBase>(100);
        }

        public void RegistToTree(PhysicsBase physicsBase)
        {
            mPrimList.Add(physicsBase);
        }

        public void UnregistFromTree(PhysicsBase physicsBase)
        {
            mPrimList.Remove(physicsBase);
        }

        public void BuildTree(int leafCount = 2)
        {
            var buildInfoDict = new Dictionary<Node, NodeBuildInfo>(mPrimList.Count);
            mRoot = new Node();
            buildInfoDict[mRoot] = new NodeBuildInfo() { ElementStartIndex = 0, ElementEndIndex = mPrimList.Count - 1 };

            var taskQueue = new Queue<Node>();
            taskQueue.Enqueue(mRoot);

            var mCountStatistics = 0;
            var error = 0;
            while (taskQueue.Count > 0)
            {
                if (error > 100000)
                {
                    UnityEngine.Debug.LogError("while error!");
                    break;
                }

                error++;
                mCountStatistics++;

                var currentNode = taskQueue.Dequeue();
                var bb = default(Bounds?);
                var bc = default(Bounds?);

                var buildInfo = buildInfoDict[currentNode];

                for (int i = buildInfo.ElementStartIndex; i <= buildInfo.ElementEndIndex; i++)
                {
                    var item = mPrimList[i];

                    if (bb == null)
                    {
                        bb = item.PhysicsShape.Bounds;
                    }
                    else
                    {
                        var bb_value = bb.Value;
                        bb_value.Encapsulate(item.PhysicsShape.Bounds);
                        bb = bb_value;
                    }

                    if (bc == null)
                    {
                        bc = new Bounds(item.PhysicsShape.GetWorldCenter(), Vector3.zero);
                    }
                    else
                    {
                        var bc_value = bc.Value;
                        bc_value.Encapsulate(item.PhysicsShape.GetWorldCenter());
                        bc = bc_value;
                    }
                }
                currentNode.Bounds = bb.GetValueOrDefault();

                //var go = new GameObject(mCountStatistics.ToString());
                //var test_bc = go.AddComponent<BoxCollider>();
                //test_bc.center = bb.Value.center;
                //test_bc.size = bb.Value.size;

                if (buildInfo.ElementEndIndex - buildInfo.ElementStartIndex <= leafCount - 1)
                {
                    currentNode.ItemsArray = new PhysicsBase[(buildInfo.ElementEndIndex - buildInfo.ElementStartIndex) + 1];
                    for (int i = buildInfo.ElementStartIndex, j = 0; i <= buildInfo.ElementEndIndex; i++, j++)
                    {
                        currentNode.ItemsArray[j] = mPrimList[i];
                    }
                    continue;
                }

                var axis = 0;
                var diff = bb.Value.max - bb.Value.min;
                if (diff.y > diff.x && diff.y > diff.z)
                    axis = 1;
                else if (diff.z > diff.y && diff.z > diff.x)
                    axis = 2;
                else if (diff.x > diff.y && diff.x > diff.z)
                    axis = 0;

                var midValue = bc.Value.min[axis] + ((bc.Value.max[axis] - bc.Value.min[axis]) * 0.5f);
                var midIndex = buildInfo.ElementStartIndex;

                for (int i = buildInfo.ElementStartIndex; i <= buildInfo.ElementEndIndex; i++)
                {
                    var item = mPrimList[i];
                    if (item.PhysicsShape.GetWorldCenter()[axis] < midValue)
                    {
                        mPrimList[i] = mPrimList[midIndex];
                        mPrimList[midIndex] = item;

                        midIndex++;
                    }
                }

                for (int i = buildInfo.ElementStartIndex; i <= midIndex; i++)
                {
                    for (int j = i; j <= midIndex; j++)
                    {
                        if (i == j) continue;

                        if (mPrimList[i].PhysicsShape.GetWorldCenter()[axis] > mPrimList[j].PhysicsShape.GetWorldCenter()[axis])
                        {
                            var temp = mPrimList[j];
                            mPrimList[j] = mPrimList[i];
                            mPrimList[i] = temp;
                        }
                    }
                }

                if (midIndex == buildInfo.ElementStartIndex || midIndex == buildInfo.ElementEndIndex)
                {
                    midIndex = buildInfo.ElementStartIndex + (buildInfo.ElementEndIndex - buildInfo.ElementStartIndex) / 2;
                }

                currentNode.LChildren = new Node();
                buildInfoDict[currentNode.LChildren] = new NodeBuildInfo() { ElementStartIndex = buildInfo.ElementStartIndex, ElementEndIndex = midIndex };
                taskQueue.Enqueue(currentNode.LChildren);

                currentNode.RChildren = new Node();
                buildInfoDict[currentNode.RChildren] = new NodeBuildInfo() { ElementStartIndex = midIndex + 1, ElementEndIndex = buildInfo.ElementEndIndex };
                taskQueue.Enqueue(currentNode.RChildren);
            }

            taskQueue.Clear();
            taskQueue.TrimExcess();
            buildInfoDict.Clear();
            mQueryCacheQueue = new Queue<Node>(mCountStatistics);
        }

        public void ClearPrimList()
        {
            mPrimList.Clear();
            mPrimList.TrimExcess();
        }

        public PhysicsBase ContainPointFirst(Vector3 point)
        {
            mQueryCacheQueue.Clear();
            mQueryCacheQueue.Enqueue(mRoot);

            while (mQueryCacheQueue.Count > 0)
            {
                var item = mQueryCacheQueue.Dequeue();

                if (item.Bounds.Contains(point))
                {
                    if (item.IsLeaf)
                    {
                        var itemsArray = item.ItemsArray;
                        for (int i = 0, iMax = itemsArray.Length; i < iMax; i++)
                        {
                            var leafItem = itemsArray[i];
                            if (leafItem.PhysicsShape.IsContainPoint(point))
                                return leafItem;
                        }
                    }
                    else
                    {
                        mQueryCacheQueue.Enqueue(item.LChildren);
                        mQueryCacheQueue.Enqueue(item.RChildren);
                    }
                }
            }

            return null;
        }

        public int ContainPoint(Vector3 point, PhysicsBase[] cachedResult)
        {
            mQueryCacheQueue.Clear();
            mQueryCacheQueue.Enqueue(mRoot);

            var resultIndex = 0;
            while (mQueryCacheQueue.Count > 0)
            {
                var item = mQueryCacheQueue.Dequeue();

                if (item.Bounds.Contains(point))
                {
                    if (item.IsLeaf)
                    {
                        var itemsArray = item.ItemsArray;
                        for (int i = 0, iMax = itemsArray.Length; i < iMax; i++)
                        {
                            if (resultIndex + 1 < cachedResult.Length)
                            {
                                cachedResult[resultIndex] = itemsArray[i];
                                resultIndex++;
                            }
                            else
                            {
                                mQueryCacheQueue.Clear();
                                break;
                            }
                        }
                    }
                    else
                    {
                        mQueryCacheQueue.Enqueue(item.LChildren);
                        mQueryCacheQueue.Enqueue(item.RChildren);
                    }
                }
            }

            return resultIndex;
        }

        public int Intersect(Bounds bounds, PhysicsBase[] cachedResult)
        {
            mQueryCacheQueue.Clear();
            mQueryCacheQueue.Enqueue(mRoot);

            var resultIndex = 0;
            while (mQueryCacheQueue.Count > 0)
            {
                var item = mQueryCacheQueue.Dequeue();

                if (item.Bounds.Intersects(bounds))
                {
                    if (item.IsLeaf)
                    {
                        var itemsArray = item.ItemsArray;
                        for (int i = 0, iMax = itemsArray.Length; i < iMax; i++)
                        {
                            if (resultIndex + 1 < cachedResult.Length)
                            {
                                cachedResult[resultIndex] = itemsArray[i];
                                resultIndex++;
                            }
                            else
                            {
                                mQueryCacheQueue.Clear();
                                break;
                            }
                        }
                    }
                    else
                    {
                        mQueryCacheQueue.Enqueue(item.LChildren);
                        mQueryCacheQueue.Enqueue(item.RChildren);
                    }
                }
            }

            return resultIndex;
        }

        public int IntersectLine(Vector3 beginPoint, Vector3 endPoint, PhysicsBase[] cachedResult)
        {
            var d = endPoint - beginPoint;
            return IntersectRay(new Ray(beginPoint, d.normalized), cachedResult, d.magnitude);
        }

        public int IntersectRay(Ray ray, PhysicsBase[] cachedResult, float? limitDistance = null)
        {
            mQueryCacheQueue.Clear();
            mQueryCacheQueue.Enqueue(mRoot);

            var resultIndex = 0;
            while (mQueryCacheQueue.Count > 0)
            {
                var item = mQueryCacheQueue.Dequeue();

                float distance = 0f;
                if (item.Bounds.IntersectRay(ray, out distance) && (limitDistance != null && distance <= limitDistance.Value || limitDistance == null))
                {
                    if (item.IsLeaf)
                    {
                        var itemsArray = item.ItemsArray;
                        for (int i = 0, iMax = itemsArray.Length; i < iMax; i++)
                        {
                            if (resultIndex + 1 < cachedResult.Length)
                            {
                                cachedResult[resultIndex] = itemsArray[i];
                                resultIndex++;
                            }
                            else
                            {
                                mQueryCacheQueue.Clear();
                                break;
                            }
                        }
                    }
                    else
                    {
                        mQueryCacheQueue.Enqueue(item.LChildren);
                        mQueryCacheQueue.Enqueue(item.RChildren);
                    }
                }
            }

            return resultIndex;
        }
    }
}
